﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace Geometry {
    public class Matrix44 {
        // 0,0 0,1 0,2 0,3
        // 1,0 1,1 1,2 1,3
        // 2,0 2,1 2,2 2,3
        // 3,0 3,1 3,2 3,3
        public float[][] m;

        public Matrix44() {
            build();
            for (int i = 0; i < m.Length; ++i)
                for (int j = 0; j < m[0].Length; ++j)
                    m[i][j] = (i == j ? 1.0f : 0.0f);
        }
        public Matrix44(
            float n00, float n01, float n02, float n03,
            float n10, float n11, float n12, float n13,
            float n20, float n21, float n22, float n23,
            float n30, float n31, float n32, float n33) {
            build();
            m[0][0] = n00; m[0][1] = n01; m[0][2] = n02; m[0][3] = n03;
            m[1][0] = n10; m[1][1] = n11; m[1][2] = n12; m[1][3] = n13;
            m[2][0] = n20; m[2][1] = n21; m[2][2] = n22; m[2][3] = n23;
            m[3][0] = n30; m[3][1] = n31; m[3][2] = n32; m[3][3] = n33;
        }
        public Matrix44(Matrix44 n) {
            build();
            for (int i = 0; i < m.Length; ++i)
                for (int j = 0; j < m[i].Length; ++j)
                    m[i][j] = n.m[i][j];
        }

        public static Matrix44 operator *(Matrix44 a, Matrix44 b) {
            Matrix44 res = new Matrix44();
            for (int row = 0; row < 4; ++row)
                for (int col = 0; col < 4; ++col)
                    res.m[row][col] =
                        a.m[row][0] * b.m[0][col] +
                        a.m[row][1] * b.m[1][col] +
                        a.m[row][2] * b.m[2][col] +
                        a.m[row][3] * b.m[3][col];
            return res;
        }

        public Matrix44 transpose() {
            return new Matrix44(
                m[0][0], m[1][0], m[2][0], m[3][0],
                m[0][1], m[1][1], m[2][1], m[3][1],
                m[0][2], m[1][2], m[2][2], m[3][2],
                m[0][3], m[1][3], m[2][3], m[3][3]);
        }

        public void build() {
            m = new float[4][];
            for (int i = 0; i < m.Length; ++i) {
                m[i] = new float[4];
            }
        }
        public Matrix44 inverse() {
            Matrix44 clone = new Matrix44(this);
            Matrix44 minv = new Matrix44(
                1.0f, 0.0f, 0.0f, 0.0f,
                0.0f, 1.0f, 0.0f, 0.0f,
                0.0f, 0.0f, 1.0f, 0.0f,
                0.0f, 0.0f, 0.0f, 1.0f);
            int[] irow = { 0, 1, 2, 3 };

            for (int col = 0; col < 4; ++col) {
                float max = Math.Abs(clone.m[irow[col]][col]);
                int ipiv = col;
                // select pivot
                for (int row = col + 1; row < 4; ++row) {
                    float tmp = Math.Abs(clone.m[irow[row]][col]);
                    if (tmp > max) {
                        ipiv = row;
                        max = tmp;
                    }
                }
                if (max == 0.0f)
                    throw new Exception("singlar matrix");
                if (col != ipiv) {
                    int itmp = irow[col];
                    irow[col] = irow[ipiv];
                    irow[ipiv] = itmp;
                }

                // subtract
                float invPivot = 1.0f / clone.m[irow[col]][col];
                for (int i = 0; i < 4; ++i) {
                    clone.m[irow[col]][i] *= invPivot;
                    minv.m[irow[col]][i] *= invPivot;
                }
                for (int row = 0; row < 4; ++row) {
                    if (row == col)
                        continue;

                    float[] targetRow = clone.m[irow[row]];
                    float targetRowVal = targetRow[col];
                    float[] pivotCloneRow = clone.m[irow[col]];
                    float[] pivotInvRow = minv.m[irow[col]];
                    for (int i = 0; i < 4; ++i) {
                        float ftmp;
                        ftmp = targetRowVal * pivotCloneRow[i];
                        clone.m[irow[row]][i] -= ftmp;

                        ftmp = targetRowVal * pivotInvRow[i];
                        minv.m[irow[row]][i] -= ftmp;

                    }
                }

            }
            float[][] swaped = { minv.m[irow[0]], minv.m[irow[1]], minv.m[irow[2]], minv.m[irow[3]] };
            minv.m = swaped;

            return minv;
        }
    }
}
